#include "IMU_Task.h"

IMU_Angle_t EulerAngle_Actual;//欧拉角，实际值
IMU_Angle_t EulerAngle_Target;//欧拉角，目标值

Q_info_Typedef Q_info_Actual; // 全局四元数，实际值
Q_info_Typedef Q_info_Target; // 全局四元数，目标值
Q_info_Typedef Q_info_Error; // 全局四元数，误差值
Q_info_Typedef Q_info_Error_Linear;// 全局四元数，误差值线性化
Q_info_Typedef Q_info_Target_instance;// 全局四元数的逆，目标值

Matrix_to_R_Typdef Matrix_Actual;  //旋转矩阵，实际值
Matrix_to_R_Typdef Matrix_Target;  //旋转矩阵，目标值
Matrix_to_R_Typdef Matrix_Error;  //旋转矩阵，误差值
Matrix_to_R_Typdef Matrix_Target_Transpose;  //旋转矩阵的转置，目标值

Axial_Typdef Axial_Err;  //轴角,旋转误差

void IMU_EKF_Task(void)
{
    ICM20602_GetValue();
    IMU_QuaternionEKF_Update(ICM_Data.GYRO[0],ICM_Data.GYRO[1],ICM_Data.GYRO[2],
							ICM_Data.ACCL[0],ICM_Data.ACCL[1],ICM_Data.ACCL[2]);
    EulerAngle_Actual.Pitch=Get_Pitch(); //获得pitch
    EulerAngle_Actual.Roll=Get_Roll();//获得roll
    EulerAngle_Actual.Yaw=Get_Yaw();//获得yaw
    EulerAngle_Actual.YawTotalAngle = Get_YawTotalAngle();
    Get_Q(&Q_info_Actual.q0 , &Q_info_Actual.q1 , &Q_info_Actual.q2 , &Q_info_Actual.q3);
}
